#ifndef COMMON_MATH_CUBIC_INTERPOLATION_
#define COMMON_MATH_CUBIC_INTERPOLATION_

#include <Eigen/Dense>
#include <vector>
#include "common/common.h"
namespace vehicle {
namespace common {
namespace math {
class CubicSpline {
 private:
  std::vector<double> x_;
  std::vector<double> y_;
  int num_point_;
  std::vector<double> coefficient0_;
  std::vector<double> coefficient1_;
  std::vector<double> coefficient2_;
  std::vector<double> coefficient3_;

 public:
  CubicSpline(std::vector<double>, std::vector<double>);
  CubicSpline() = default;
  ~CubicSpline() = default;

  void Build(const std::vector<double>& x, const std::vector<double>& y);
  double EvaluateCubicSpline(const double& x);
  double EvaluateFirstDerivative(const double& x);
  double EvaluateSecondDerivative(const double& x);
};


class CubicSpline2D {

public:
    CubicSpline2D() = default;
    ~CubicSpline2D() = default;


private:
    CubicSpline sx;
    CubicSpline sy;
    std::vector<double> s;
    std::vector<double> x_;
    std::vector<double> y_;
    std::vector<double> heading_;
    std::vector<double> curvature_;
public:
    double init(std::vector<double> x, std::vector<double> y);
    void calc_s(std::vector<double> &x,std::vector<double> &y,std::vector<double> &s);
    void calc_position(double &s,double &x,double &y);
    void calc_curvature(double &s,double &k);
    void calc_yaw(double &s,double &heading);
    double get_s(float x,float y, float heading);
    std::vector<GPSPoint> GetGpsPoints();

};
}}}
#endif
